#ifndef FIRSTORDERLOWPASSFILTER_H
#define FIRSTORDERLOWPASSFILTER_H

template <typename T>
class FirstOrderLowPassFilter {
 public:
  FirstOrderLowPassFilter() : is_init_(false){};
  ~FirstOrderLowPassFilter(){};

  /// @brief add measurement
  void AddMeasurement(T& x, float alpha) noexcept {
    if (is_init_) {
      state_ = x + alpha * (state_ - x);
    } else {
      state_ = x;
      is_init_ = true;
    }
  };

  /// @brief get state
  T GetState() noexcept { return state_; };

 private:
  bool is_init_;
  T state_;
};

#endif  // FIRSTORDERLOWPASSFILTER_H
